import pyrealsense2 as rs
import numpy as np
import cv2
import pyttsx3 as pts

# 函数：在图像上绘制网格并显示距离值
def draw_grid_with_distance(img, num_cols, num_rows, depth_frame):
    # 获取图像的高度和宽度
    height, width = img.shape[:2]
    # 计算网格的高度和宽度
    grid_height = height // num_rows
    grid_width = width // num_cols

    # 遍历网格行和列
    for i in range(num_rows):
        for j in range(num_cols):
            # 计算网格左上角坐标
            x1 = j * grid_width
            y1 = i * grid_height
            # 计算网格右下角坐标
            x2 = (j + 1) * grid_width
            y2 = (i + 1) * grid_height
            # 计算网格中心坐标
            cx = (x1 + x2) // 2
            cy = (y1 + y2) // 2
            # 获取网格中心处的深度值
            depth_value = depth_frame.get_distance(cx, cy)
            # 检查深度值是否小于0.7m
            if depth_value < 0.7:
                # 记录位置
                close_positions.append((cx, cy))
                # 计数
                close_count[0] += 1
            # 在网格中心绘制距离值
            cv2.putText(img, "{:.2f}m".format(depth_value), (cx - 30, cy), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
            # 绘制网格线
            cv2.rectangle(img, (x1, y1), (x2, y2), (255, 255, 255), 1)
if __name__ == "__main__":
    # 初始化计数器
    close_count = [0]
    # 初始坐标记录器
    close_positions = []
    # 创建 RealSense 管道对象
    pipeline = rs.pipeline()
    config = rs.config()
    # 配置管道以获取深度和彩色图像流
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
    # 启动管道
    pipeline.start(config)

    try:
        while True:
            # 等待一帧数据
            frames = pipeline.wait_for_frames()
            # 获取深度和彩色帧
            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            # 如果没有有效帧，则继续循环
            if not depth_frame or not color_frame:
                continue

            # 将深度帧和彩色帧转换为 numpy 数组
            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())

            #重置
            close_count[0]=0
            close_positions.clear()

            # 在深度图像上绘制网格并获取小于0.7m网格的位置
            draw_grid_with_distance(depth_image, 10, 10, depth_frame)
            #draw_grid_with_distance(color_image, 10, 10, depth_frame)

            if close_count[0] > 20:
                # 判断小于0.7m的网格是否大于等于50%在摄像头中心点下方
                center_y = depth_image.shape[0]
                below_center_count = sum(1 for x, y in close_positions if y > center_y)
                if len(close_positions) > 0 and below_center_count / len(close_positions) >= 0.5:
                    #pt = pts.init()
                    #pt.say('注意，下方有障碍！')
                    #pt.runAndWait()
                    print("注意，下方有障碍！")

            # 将深度图像转换为伪彩色图像
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
            # 将彩色图像和深度伪彩色图像水平堆叠以显示在同一窗口中
            images = np.hstack((color_image, depth_colormap))

            # 创建窗口并显示图像
            cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
            cv2.imshow('RealSense', images)
            # 检测键盘输入，如果是 'q' 键或者 ESC 键则退出循环
            key = cv2.waitKey(1)
            if key & 0xFF == ord('q') or key == 27:
                cv2.destroyAllWindows()
                break
    finally:
        # 停止
        pipeline.stop()
